#include <ros/ros.h>
#include <geometry_msgs/PoseStamped.h>
#include <trajectory_msgs/JointTrajectoryPoint.h>
#include <Eigen/Eigen>
#include <mavros_msgs/State.h>
#include <mavros_msgs/CommandBool.h>
#include <mavros_msgs/SetMode.h>
#include <fstream>
#include <sstream>
#include <string>
#include <vector>
#include <math.h>

using namespace Eigen;
using namespace std;

Vector3d current_p;
Vector3d last_position;
mavros_msgs::State current_state;
ros::Publisher pva_pub;
ros::ServiceClient arming_client;
ros::ServiceClient set_mode_client;

void positionCallback(const geometry_msgs::PoseStamped::ConstPtr &msg) {
    current_p << msg->pose.position.x, msg->pose.position.y, msg->pose.position.z;
}

void stateCallback(const mavros_msgs::State::ConstPtr &msg) {
    current_state = *msg;
}

void setPVA(Eigen::Vector3d p, Eigen::Vector3d v, Eigen::Vector3d a, double yaw = 0.0) {
    trajectory_msgs::JointTrajectoryPoint pva_setpoint;
    pva_setpoint.positions = {p(0), p(1), p(2), yaw};
    pva_setpoint.velocities = {v(0), v(1), v(2)};
    pva_setpoint.accelerations = {a(0), a(1), a(2)};

    pva_pub.publish(pva_setpoint);
}

Vector3d parseLine(const std::string& line) {
    std::stringstream ss(line);
    Vector3d pos;
    ss >> pos(0) >> pos(1) >> pos(2);
    return pos;
}

std::vector<Vector3d> readPositions(const std::string& filename) {
    std::vector<Vector3d> positions;
    std::ifstream file(filename);
    std::string line;

    if (file.is_open()) {
        while (getline(file, line)) {
            positions.push_back(parseLine(line));
        }
        file.close();
    } else {
        ROS_ERROR_STREAM("Unable to open file " << filename);
    }

    return positions;
}

int main(int argc, char **argv) {
    ros::init(argc, argv, "path_location");
    ros::NodeHandle nh;

    pva_pub = nh.advertise<trajectory_msgs::JointTrajectoryPoint>("/pva_setpoint", 1);
    arming_client = nh.serviceClient<mavros_msgs::CommandBool>("mavros/cmd/arming");
    set_mode_client = nh.serviceClient<mavros_msgs::SetMode>("mavros/set_mode");
    ros::Time last_request = ros::Time::now();

    ros::Subscriber position_sub = nh.subscribe<geometry_msgs::PoseStamped>("/mavros/vision_pose/pose", 1, positionCallback);
    ros::Subscriber state_sub = nh.subscribe<mavros_msgs::State>("/mavros/state", 1, stateCallback);

    while (ros::ok() && !current_state.connected) {
        ros::spinOnce();
        ros::Rate(1).sleep(); 
    }

    std::string line;
    ifstream file("/home/up/pathpoint_ws/src/pathpoint/curv.txt");
    if (!file.is_open()) {
        ROS_ERROR_STREAM("Unable to open file /home/up/pathpoint_ws/src/pathpoint/curv.txt");
        return -1;
    }

    while (ros::ok() && getline(file, line)) {
        if (current_state.mode != "OFFBOARD" &&
            (ros::Time::now() - last_request > ros::Duration(2.0))) {
            mavros_msgs::SetMode offb_set_mode;
            offb_set_mode.request.custom_mode = "OFFBOARD";
            if (set_mode_client.call(offb_set_mode) && offb_set_mode.response.mode_sent) {
                ROS_INFO("Offboard enabled");
            }
            last_request = ros::Time::now();

        } else if (!current_state.armed &&
                   (ros::Time::now() - last_request > ros::Duration(2.0))) {
            mavros_msgs::CommandBool arm_cmd;
            arm_cmd.request.value = true;
            if (arming_client.call(arm_cmd) && arm_cmd.response.success) {
                ROS_INFO("Vehicle armed");
            }
            last_request = ros::Time::now();

        } else if (current_state.mode == "OFFBOARD" && current_state.armed) {
            Vector3d pos = parseLine(line);
            geometry_msgs::PoseStamped pose;
            pose.pose.position.x = pos(0);
            pose.pose.position.y = pos(1);
            pose.pose.position.z = pos(2);
            pose.header.stamp = ros::Time::now();
            pva_pub.publish(pose);
        
        
        }

        ros::spinOnce();
        ros::Rate(10).sleep();
    }

    if (file.eof()) { // Check if the end of the file has been reached
        ros::Rate publish_rate(10); // Set a rate for publishing
        while (ros::ok()) {
            if (current_state.mode == "OFFBOARD" && current_state.armed) {
                geometry_msgs::PoseStamped pose;
                pose.pose.position.x = last_position(0);
                pose.pose.position.y = last_position(1);
                pose.pose.position.z = last_position(2);
                pose.header.stamp = ros::Time::now();
                pva_pub.publish(pose);
            }
            ros::spinOnce();
            publish_rate.sleep();
        }
    }

    file.close();
    return 0;
}

